#include "reDoor.h"
#include <math.h>
/* swinging_door.c */
void swinging_door_init(SwingingDoorState *state, float deviation, float delta_time) {
    state->prev_value = 0;
    state->max_slope = INFINITY;
    state->min_slope = -INFINITY;
    state->deviation = deviation;
    state->delta_time = delta_time; // 设置固定时间间隔
}

bool swinging_door_process(SwingingDoorState *state, float new_value) {
    if (state->prev_value == 0) {
        // 初始化第一个点
        state->prev_value = new_value;
        return true;
    }

    float delta_value = new_value - state->prev_value;
    float delta_time = state->delta_time; // 使用预设的固定时间间隔

    // 计算新的斜率范围
    float upper_slope = (delta_value + state->deviation) / delta_time;
    float lower_slope = (delta_value - state->deviation) / delta_time;

    // 更新斜率限制
    float new_max_slope = fminf(upper_slope, state->max_slope);
    float new_min_slope = fmaxf(lower_slope, state->min_slope);

    if (new_max_slope >= new_min_slope) {
        // 更新斜率范围，不保留当前点
        state->max_slope = new_max_slope;
        state->min_slope = new_min_slope;
        return false;
    } else {
        // 超出范围，保留前一个点，重置状态
        state->prev_value = new_value;
        state->max_slope = INFINITY;
        state->min_slope = -INFINITY;
        return true;
    }
}